#include "JPhysicsManager.h"

#include "JPhysicsObject.h"
#include "JPhysicsMaths.h"
/*
JPhysicsManager::JPhysicsManager()
{
	m_objectLLStart = 0;
	m_objectLLCurrent = 0;
	m_objectCount = 0;
}

void JPhysicsManager::initPhysics(float gravity)
{
	m_gravity = gravity; 
}

void JPhysicsManager::releasePhysics()
{
	m_objectLLCurrent = m_objectLLStart;

	while (m_objectLLCurrent != 0)
	{
		if(m_objectLLCurrent->m_physicsObject)
			delete m_objectLLCurrent->m_physicsObject;

		ObjectLinkedList *temp = m_objectLLCurrent;
		m_objectLLCurrent = m_objectLLCurrent->m_objectLLNext;
		delete temp;
	}
}

void JPhysicsManager::update(float timeDiff)
{
	ObjectLinkedList *updateLLFirst = m_objectLLStart;
	ObjectLinkedList *updateLLSecond = updateLLFirst;

	for (int i = 0; i < (m_objectCount-1); i++)
	{
		for (int j = (i + 1); j < m_objectCount; j++)
		{
			updateLLSecond = updateLLSecond->m_objectLLNext;
			collisionCheck(*updateLLFirst->m_physicsObject, *updateLLSecond->m_physicsObject); 
		}
		m_collisionHandler.updateDueToGravity(updateLLFirst->m_physicsObject, m_gravity);
		updateLLFirst->m_physicsObject->update(timeDiff);
		updateLLFirst = updateLLFirst->m_objectLLNext;
		updateLLSecond = updateLLFirst;
	}

	if(updateLLFirst) //Special case, final object in linked list (if there is more than 1)
	{
		m_collisionHandler.updateDueToGravity(updateLLFirst->m_physicsObject, m_gravity);
		updateLLFirst->m_physicsObject->update(timeDiff);
	}
	else  // special case when there is only 1 object
	{
		m_collisionHandler.updateDueToGravity(m_objectLLStart->m_physicsObject, m_gravity);
		m_objectLLStart->m_physicsObject->update(timeDiff);
	}
}

JPhysicsObject* JPhysicsManager::createPhysicsSphere(
	CollisionType collisionType,
	float mass,
	float radius)
{
	JPhysicsObject* returnSphere = new JPhysicsObject;

	returnSphere->setMass(mass);
	returnSphere->setRadius(radius);
	returnSphere->setCollisionType(collisionType);

	float inertia[3];
	for(int i=0; i<3; i++)
		inertia[i] = 0.4f*mass*radius*radius;

	returnSphere->setInertia(inertia);

	if(!m_objectLLStart)
	{
		// First object case
		m_objectLLStart = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLStart;

		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnSphere;
		m_objectCount++;
	}
	else if(!m_objectLLCurrent->m_objectLLNext)
	{
		m_objectLLCurrent->m_objectLLNext = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLCurrent->m_objectLLNext;
		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnSphere;
		m_objectCount++;
	}
	return returnSphere;
}

JPhysicsObject* JPhysicsManager::createPhysicsPlane(
	CollisionType collisionType,
	JPVector3 *pointOnPlane,
	JPVector3 *normal)
{
	JPhysicsObject* returnPlane = new JPhysicsObject;

	returnPlane->setPosition(pointOnPlane);
	returnPlane->setPlaneNormal(normal);
	returnPlane->setCollisionType(collisionType);

	if(!m_objectLLStart)
	{
		// First object case
		m_objectLLStart = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLStart;

		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnPlane;
		m_objectCount++;
	}
	else if(!m_objectLLCurrent->m_objectLLNext)
	{
		m_objectLLCurrent->m_objectLLNext = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLCurrent->m_objectLLNext;
		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnPlane;
		m_objectCount++;
	}
	return returnPlane;
}

JPhysicsObject* JPhysicsManager::createPhysicsCube(
	CollisionType collisionType,
	JPVector3 *centre,
	float mass,
	JPVector3 *orientation,
	float *halfLength)
{
	JPhysicsObject* returnCube = new JPhysicsObject;

	returnCube->setPosition(centre);
	returnCube->setMass(mass);
	returnCube->setOrientation(orientation);
	returnCube->setHalfLength(halfLength);
	returnCube->setCollisionType(collisionType);  

	returnCube->setRadius(sqrtf(halfLength[0]*halfLength[0] +
		halfLength[1]*halfLength[1] +
		halfLength[2]*halfLength[2]));

	float inertia[3];
	for(int i = 0; i < 3; i++)
	{
		inertia[i] = (mass/3.0f) * (orientation[i].x*orientation[i].x*(halfLength[1]*halfLength[1] + halfLength[2]*halfLength[2])+ 
			orientation[i].y*orientation[i].y*(halfLength[0]*halfLength[0] + halfLength[2]*halfLength[2])+
			orientation[i].z*orientation[i].z*(halfLength[0]*halfLength[0] + halfLength[1]*halfLength[1]));
	}
	returnCube->setInertia(inertia);

	if(!m_objectLLStart)
	{
		// First object case
		m_objectLLStart = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLStart;

		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnCube;
		m_objectCount++;
	}
	else if(!m_objectLLCurrent->m_objectLLNext)
	{
		m_objectLLCurrent->m_objectLLNext = new ObjectLinkedList;
		m_objectLLCurrent = m_objectLLCurrent->m_objectLLNext;
		m_objectLLCurrent->m_objectLLNext = 0;
		m_objectLLCurrent->m_physicsObject = returnCube;
		m_objectCount++;
	}
	return returnCube;
}

void JPhysicsManager::collisionCheck(const JPhysicsObject &objectOne, const JPhysicsObject &objectTwo)
{
	if(objectOne.getCollisionType() == RADIAL)
	{
		JPVector3 collisionPoint;
		JPVector3 normal;
		bool collision;

		switch(objectTwo.getCollisionType())
		{
		case PLANE:      
			collision = m_collisionHandler.spherePlaneCol(objectOne,objectTwo, collisionPoint);

			if(collision) 
			{
				//m_successfulCollision++;
				m_collisionHandler.objectPlaneResponse(objectOne,objectTwo, &collisionPoint, 0.6f);
			}
			break;

		case RADIAL:    
			collision = m_collisionHandler.sphereSphereCol(objectOne,objectTwo, collisionPoint, normal);
			if(collision)  
			{
				m_collisionHandler.objectObjectResponse(objectOne,objectTwo, &collisionPoint, &normal, 0.6f);  
			} 
			break;

		case AABB:  // Response for AABB and OBB have object's reversed as parameters
			// because the repsonse function is designed for the normal pointing towards
			// first parameter. The Sphere response functions return a normal pointing towards
			// The AABB and OBB, thus they must be the first parameter.
			collision = m_collisionHandler.AABBSphereCol(objectOne,objectTwo, collisionPoint, normal);
			if(collision)  
			{ 
				m_collisionHandler.objectObjectResponse(objectTwo,objectOne, &collisionPoint, &normal, 0.6f);  
			}   
			break;

		case OBB:
			collision = m_collisionHandler.OBBSphereCol(objectOne,objectTwo, collisionPoint, normal);
			if(collision)
			{
				m_collisionHandler.objectObjectResponse(objectTwo,objectOne, &collisionPoint, &normal, 0.6f);
			}
			break;

		default:
			printf("error\n");
			break;
		}
	}

	if(objectOne.getCollisionType() == OBB)
	{ 
		JPVector3 collisionPoint(0.0f,0.0f,0.0f);
		JPVector3 normal(0.0f,0.0f,0.0f);
		bool collision;    

		switch(objectTwo.getCollisionType())
		{
		case PLANE:      
			collision = m_collisionHandler.OBBPlaneCol(objectOne,objectTwo, collisionPoint);
			if(collision)  
			{
				//m_successfulCollision++;
				m_collisionHandler.objectPlaneResponse(objectOne, objectTwo, &collisionPoint, 0.1f);
			}
			break;

		case RADIAL:
			collision = m_collisionHandler.OBBSphereCol(objectTwo ,objectOne, collisionPoint, normal);
			if(collision)
			{
				m_collisionHandler.objectObjectResponse(objectOne,objectTwo, &collisionPoint, &normal, 0.6f);
			}
			break;

		case AABB:

			break;

		case OBB:
			m_collisionCount++;
			collision = m_collisionHandler.SAT(objectOne,objectTwo, collisionPoint, normal);
			if(collision)  
			{
				m_collisionHandler.objectObjectResponse(objectOne,objectTwo, &collisionPoint, &normal, 0.8f);    
			}
			break;

		default:
			printf("error\n");
			break;
		}    
	}
	if(objectOne.getCollisionType() == AABB)
	{ 
		JPVector3 collisionPoint(0.0f,0.0f,0.0f);
		JPVector3 normal(0.0f,0.0f,0.0f);
		bool collision;

		switch(objectTwo.getCollisionType())
		{
		case PLANE: 
			collision = m_collisionHandler.AABBPlaneCol(objectOne,objectTwo, collisionPoint);      
			if(collision)  
			{
				m_collisionHandler.objectPlaneResponse(objectOne, objectTwo, &collisionPoint, 0.1f);
			}
			break;

		case RADIAL: 
			collision = m_collisionHandler.AABBSphereCol(objectTwo,objectOne, collisionPoint, normal);
			if(collision)  
			{ 
				m_collisionHandler.objectObjectResponse(objectOne,objectTwo, &collisionPoint, &normal, 0.6f);  
			}   
			break;

		case AABB: 
			collision = m_collisionHandler.AABBAABBCol(objectOne,objectTwo, collisionPoint, normal);
			if(collision)   
			{
				m_collisionHandler.objectObjectResponse(objectOne,objectTwo, &collisionPoint, &normal, 0.6f);  
			}
			break;

		case OBB: 

			break;

		default:
			printf("error\n");
			break;
		}
	}
}
*/